<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of gmsppf</title>
  <meta name="keywords" content="gmsppf">
  <meta name="description" content="GMSPPF  Gaussian Mixture Sigma-Point Particle Filter">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; gmsppf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>gmsppf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>GMSPPF  Gaussian Mixture Sigma-Point Particle Filter</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [estimate, ParticleFilterDS, pNoise, oNoise, extra] = gmsppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> GMSPPF  Gaussian Mixture Sigma-Point Particle Filter

   [estimate, ParticleFilterDS, pNoise, oNoise] = gmsppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         ParticleFilterDS     Particle filter data structure. (see field definitions below)
         pNoise               (NoiseDS) process noise data structure  (must be of type 'gmm')
         oNoise               (NoiseDS) observation noise data structure
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          Inference data structure generated by GENINFDS function.

   OUTPUT
         estimate             State estimate generated from posterior distribution of state given all observation. Type of
                              estimate is specified by InferenceDS.estimateType
         ParticleFilterDS     Updated Particle filter data structure.
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

   ParticleFilterDS fields:
         .N                   (scalar) number of particles to use
         .stateGMM            (gmm) Gaussian mixture model of state distribution with the following field:
                  .M            (scalar) number of mixture components in GMM
                  .mu           (statedim-by-M) buffer of mean vectors (centroids) of state GMM components
                  .cov          (statedim-by-statedim-my-M) buffer of covariance matrices of state GMM components
                  .cov_type     (string) covariance matrix type ('full','sqrt','diag','swrt-diag') 'sqrt' is preferred.
                  .weights      (1-by-M) state GMM component weights (priors)

   Required InferenceDS fields:
         .spkfType            (string) Type of SPKF to use (srukf or srcdkf).
         .estimateType        (string) Estimate type : 'mean', 'mode', etc.

   NOTE : All covariances are assumed to be of type 'sqrt', i.e. Cholesky factors.

   See also
   <a href="pf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = pf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">PF</a>, <a href="sppf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = sppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">SPPF</a>, <a href="gspf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = gspf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">GSPF</a>
   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="gmmfit.html" class="code" title="function [gmmDS, leb] = gmmfit(X, M, tt, cov_type, check_cov, display, W)">gmmfit</a>	GMMFIT   Fit a Gaussian mixture model (GMM) with M components to dataset X</li><li><a href="gmmprobability.html" class="code" title="function [prior, likelihood, evidence, posterior] = gmmprobability(gmmDS, X, W)">gmmprobability</a>	GMMPROBABILITY  Calculates any of the related (through Bayes rule) probabilities</li><li><a href="gmmsample.html" class="code" title="function [X,comp] = gmmsample(gmmDS, N)">gmmsample</a>	GMMSAMPLE  Draw N samples from the Gaussian mixture model (GMM) described by the</li><li><a href="residualresample.html" class="code" title="function outIndex = residualResample(inIndex, weights);">residualresample</a>	RESIDUALRESAMPLE  Residual resampling for SIR. Performs the resampling stage of</li><li><a href="srcdkf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">srcdkf</a>	SRCDKF  Square Root Central Difference Kalman Filter (Sigma-Point Kalman Filter variant)</li><li><a href="srukf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">srukf</a>	SRUKF  Square Root Unscented Kalman Filter (Sigma-Point Kalman Filter variant)</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse4.html" class="code" title="">demse4</a>	DEMSE4  Bearing Only Tracking Example</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse5.html" class="code" title="">demse5</a>	DEMSE4  Bearing and Frequency Tracking Example</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [estimate, ParticleFilterDS, pNoise, oNoise, extra] = gmsppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% GMSPPF  Gaussian Mixture Sigma-Point Particle Filter</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [estimate, ParticleFilterDS, pNoise, oNoise] = gmsppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         ParticleFilterDS     Particle filter data structure. (see field definitions below)</span>
0018 <span class="comment">%         pNoise               (NoiseDS) process noise data structure  (must be of type 'gmm')</span>
0019 <span class="comment">%         oNoise               (NoiseDS) observation noise data structure</span>
0020 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0021 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0022 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0023 <span class="comment">%         InferenceDS          Inference data structure generated by GENINFDS function.</span>
0024 <span class="comment">%</span>
0025 <span class="comment">%   OUTPUT</span>
0026 <span class="comment">%         estimate             State estimate generated from posterior distribution of state given all observation. Type of</span>
0027 <span class="comment">%                              estimate is specified by InferenceDS.estimateType</span>
0028 <span class="comment">%         ParticleFilterDS     Updated Particle filter data structure.</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%   ParticleFilterDS fields:</span>
0033 <span class="comment">%         .N                   (scalar) number of particles to use</span>
0034 <span class="comment">%         .stateGMM            (gmm) Gaussian mixture model of state distribution with the following field:</span>
0035 <span class="comment">%                  .M            (scalar) number of mixture components in GMM</span>
0036 <span class="comment">%                  .mu           (statedim-by-M) buffer of mean vectors (centroids) of state GMM components</span>
0037 <span class="comment">%                  .cov          (statedim-by-statedim-my-M) buffer of covariance matrices of state GMM components</span>
0038 <span class="comment">%                  .cov_type     (string) covariance matrix type ('full','sqrt','diag','swrt-diag') 'sqrt' is preferred.</span>
0039 <span class="comment">%                  .weights      (1-by-M) state GMM component weights (priors)</span>
0040 <span class="comment">%</span>
0041 <span class="comment">%   Required InferenceDS fields:</span>
0042 <span class="comment">%         .spkfType            (string) Type of SPKF to use (srukf or srcdkf).</span>
0043 <span class="comment">%         .estimateType        (string) Estimate type : 'mean', 'mode', etc.</span>
0044 <span class="comment">%</span>
0045 <span class="comment">%   NOTE : All covariances are assumed to be of type 'sqrt', i.e. Cholesky factors.</span>
0046 <span class="comment">%</span>
0047 <span class="comment">%   See also</span>
0048 <span class="comment">%   PF, SPPF, GSPF</span>
0049 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0050 <span class="comment">%</span>
0051 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0052 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0053 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0054 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0055 <span class="comment">%</span>
0056 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0057 <span class="comment">%   detail.</span>
0058 
0059 <span class="comment">%=============================================================================================</span>
0060 
0061 <span class="keyword">if</span> (nargin ~= 7) error(<span class="string">' [ gmsppf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0062 
0063 <span class="keyword">switch</span> pNoise.ns_type
0064 <span class="keyword">case</span> <span class="string">'gmm'</span>
0065 <span class="keyword">otherwise</span>
0066   error(<span class="string">' [ gmsppf ] Process noise source must be of type : gmm (Gaussian Mixture Model)'</span>);
0067 <span class="keyword">end</span>
0068 
0069 <span class="keyword">switch</span> oNoise.ns_type
0070 <span class="keyword">case</span> <span class="string">'gmm'</span>
0071 <span class="keyword">otherwise</span>
0072   error(<span class="string">' [ gmsppf ] Observation noise source must be of type : gmm (Gaussian Mixture Model)'</span>);
0073 <span class="keyword">end</span>
0074 
0075 Xdim  = InferenceDS.statedim;                            <span class="comment">% extract state dimension</span>
0076 Odim  = InferenceDS.obsdim;                              <span class="comment">% extract observation dimension</span>
0077 U1dim = InferenceDS.U1dim;                               <span class="comment">% extract exogenous input 1 dimension</span>
0078 U2dim = InferenceDS.U2dim;                               <span class="comment">% extract exogenous input 2 dimension</span>
0079 Vdim  = InferenceDS.Vdim;                                <span class="comment">% extract process noise dimension</span>
0080 Ndim  = InferenceDS.Ndim;                                <span class="comment">% extract observation noise dimension</span>
0081 
0082 numP = ParticleFilterDS.N;            <span class="comment">% number of particles to use for SIR</span>
0083 
0084 stateGMM = ParticleFilterDS.stateGMM;
0085 
0086 G    = stateGMM.M;      <span class="comment">% number of components in state GMM</span>
0087 K    = pNoise.M;        <span class="comment">% number of components in process noise GMM</span>
0088 R    = oNoise.M;        <span class="comment">% number of components in observation noise GMM</span>
0089 
0090 GK  = G*K;
0091 GKR = GK*R;
0092 
0093 stateWPrior   = zeros(1,GK);
0094 stateMuPrior  = zeros(Xdim,GK);
0095 stateCovPrior = zeros(Xdim,Xdim,GK);
0096 stateWNew     = zeros(1,GKR);
0097 stateMuNew    = zeros(Xdim,GKR);
0098 stateCovNew   = zeros(Xdim,Xdim,GKR);
0099 
0100 stateMu  = stateGMM.mu;
0101 stateCov = stateGMM.cov;
0102 stateW   = stateGMM.weights;
0103 
0104 pNoiseW  = pNoise.weights;
0105 oNoiseW  = oNoise.weights;
0106 
0107 cov_type = stateGMM.cov_type;
0108 
0109 <span class="keyword">switch</span> cov_type
0110 <span class="keyword">case</span> {<span class="string">'full'</span>,<span class="string">'diag'</span>}
0111   error(<span class="string">' [ gspf ] Currently the GSPF algorithm only support state GMMs which has ''sqrt'' covariance types.'</span>);
0112 <span class="keyword">end</span>
0113 
0114 
0115 ones_numP = ones(numP,1);
0116 ones_Xdim = ones(1,Xdim);
0117 ones_GK   = ones(GK,1);
0118 ones_GKR  = ones(GKR,1);
0119 
0120 NOV = size(obs,2);                                       <span class="comment">% number of input vectors</span>
0121 
0122 <span class="keyword">if</span> (U1dim==0), UU1=zeros(0,numP); Utemp1=[]; <span class="keyword">end</span>
0123 <span class="keyword">if</span> (U2dim==0), UU2=zeros(0,numP); Utemp2=[]; <span class="keyword">end</span>
0124 
0125 estimate   = zeros(Xdim,NOV);
0126 
0127 normfactO = (2*pi)^(Odim/2);
0128 
0129 pNoiseSPKF = struct(<span class="string">'mu'</span>,zeros(Vdim,1),<span class="string">'cov'</span>,zeros(Vdim),<span class="string">'adaptMethod'</span>,[]);
0130 oNoiseSPKF = struct(<span class="string">'mu'</span>,zeros(Ndim,1),<span class="string">'cov'</span>,zeros(Ndim),<span class="string">'adaptMethod'</span>,[]);
0131 
0132 <span class="keyword">if</span> (nargout &gt; 4)
0133   extra.mu  = zeros(Xdim,G,NOV);
0134   extra.cov = zeros(Xdim,Xdim,G,NOV);
0135   extra.weights = zeros(1,G,NOV);
0136 <span class="keyword">end</span>
0137 
0138 
0139 <span class="comment">%================================================================================================</span>
0140 <span class="comment">%--- MAIN LOOP over all data vectors</span>
0141 
0142 <span class="keyword">for</span> i=1:NOV,
0143 
0144     OBStemp = obs(:,i);                <span class="comment">% inline cvecrep</span>
0145     OBS = OBStemp(:,ones_numP);
0146 
0147     <span class="keyword">if</span> U1dim
0148       Utemp1 = U1(:,i);
0149       UU1 = Utemp1(:,ones_numP);        <span class="comment">% inline cvecrep</span>
0150     <span class="keyword">end</span>
0151     <span class="keyword">if</span> U2dim
0152       Utemp2 = U2(:,i);
0153       UU2 = Utemp2(:,ones_numP);        <span class="comment">% inline cvecrep</span>
0154     <span class="keyword">end</span>
0155 
0156 
0157     <span class="comment">%-----------------------------------------------------------------------</span>
0158     <span class="comment">% TIME UPDATE</span>
0159 
0160     <span class="keyword">for</span> r=1:R,
0161         oNoiseSPKF.mu = oNoise.mu(:,r);
0162         oNoiseSPKF.cov = oNoise.cov(:,:,r);
0163         <span class="keyword">for</span> k=1:K,
0164             pNoiseSPKF.mu  = pNoise.mu(:,k);
0165             pNoiseSPKF.cov = pNoise.cov(:,:,k);
0166             <span class="keyword">for</span> g=1:G,
0167 
0168                 a = g + (k-1)*G;
0169                 j = a + (r-1)*(GK);
0170 
0171                 <span class="keyword">switch</span> InferenceDS.spkfType
0172                 <span class="keyword">case</span> <span class="string">'srukf'</span>
0173                   [stateMuNew(:,j),stateCovNew(:,:,j),pNoiseSPKF,oNoiseSPKF,intvarDS] = <span class="keyword">...</span>
0174                     <a href="srukf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">srukf</a>(stateMu(:,g), stateCov(:,:,g), pNoiseSPKF, oNoiseSPKF, OBStemp, Utemp1, Utemp2, InferenceDS);
0175                 <span class="keyword">case</span> <span class="string">'srcdkf'</span>
0176                   [stateMuNew(:,j),stateCovNew(:,:,j),pNoiseSPKF,oNoiseSPKF,intvarDS] = <span class="keyword">...</span>
0177                     <a href="srcdkf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">srcdkf</a>(stateMu(:,g), stateCov(:,:,g), pNoiseSPKF, oNoiseSPKF, OBStemp, Utemp1, Utemp2, InferenceDS);
0178                 <span class="keyword">otherwise</span>
0179                     error(<span class="string">' [ gmsppf ] Unknown SPKF type.'</span>);
0180                 <span class="keyword">end</span>
0181 
0182                 stateMuPrior(:,a)    = intvarDS.xh_;
0183                 stateCovPrior(:,:,a) = intvarDS.Sx_;
0184                 inov = intvarDS.inov;
0185                 S    = intvarDS.Sinov;
0186                 stateWPrior(1,a) = stateW(1,g)*pNoiseW(1,k);
0187 
0188                 foo1 = S \ inov;
0189                 foo2 = exp(-0.5*foo1'*foo1) / abs(normfactO*prod(diag(S))) + 1e-99;
0190 
0191                 stateWNew(1,j)   = stateWPrior(1,a)*oNoiseW(1,r) * foo2;
0192 
0193             <span class="keyword">end</span>
0194         <span class="keyword">end</span>
0195     <span class="keyword">end</span>
0196 
0197     stateWPrior = stateWPrior / sum(stateWPrior);
0198     stateWNew   = stateWNew / sum(stateWNew);
0199 
0200 
0201     <span class="comment">%-----------------------------------------------------------------------</span>
0202     <span class="comment">% MEASUREMENT UPDATE</span>
0203 
0204     <span class="comment">% build temporary state GMM's</span>
0205     priorStateGMM = struct(<span class="string">'cov_type'</span>,cov_type,<span class="string">'mu'</span>,stateMuPrior,<span class="string">'cov'</span>,stateCovPrior,<span class="string">'weights'</span>,stateWPrior,<span class="string">'dim'</span>,Xdim,<span class="string">'M'</span>,GK);
0206     newStateGMM = struct(<span class="string">'cov_type'</span>,cov_type,<span class="string">'mu'</span>,stateMuNew,<span class="string">'cov'</span>,stateCovNew,<span class="string">'weights'</span>,stateWNew,<span class="string">'dim'</span>,Xdim,<span class="string">'M'</span>,GKR);
0207 
0208     <span class="comment">% Draw samples from the Gaussian Mixture proposal</span>
0209     XsampleBuf = <a href="gmmsample.html" class="code" title="function [X,comp] = gmmsample(gmmDS, N)">gmmsample</a>(newStateGMM,numP);
0210 
0211     <span class="comment">% evaluate likelihood of each particle under the transition prior (have to average over distribution of X_k-1)</span>
0212     [p1,p2,prior] = <a href="gmmprobability.html" class="code" title="function [prior, likelihood, evidence, posterior] = gmmprobability(gmmDS, X, W)">gmmprobability</a>(priorStateGMM, XsampleBuf);
0213 
0214     <span class="comment">% calculate observation likelihood for each particle</span>
0215     likelihood = InferenceDS.likelihood( InferenceDS, OBS, XsampleBuf, UU2, oNoise) + 1e-99;
0216 
0217     <span class="comment">% evaluate likelihood of each particle under the proposal density</span>
0218     [p1,p2,proposal] = <a href="gmmprobability.html" class="code" title="function [prior, likelihood, evidence, posterior] = gmmprobability(gmmDS, X, W)">gmmprobability</a>(newStateGMM, XsampleBuf);
0219 
0220     <span class="comment">% calculate importance weights</span>
0221     sampleW = (likelihood.*prior)./proposal;
0222     sampleW = sampleW./sum(sampleW);
0223 
0224 
0225     <span class="comment">%-----------------------------------------------------------------------</span>
0226     <span class="comment">% CALCULATE ESTIMATE</span>
0227 
0228 
0229     <span class="comment">%switch InferenceDS.estimateType</span>
0230     <span class="comment">%case 'mean'</span>
0231     <span class="comment">%    estimate(:,i) = XsampleBuf*sampleW';</span>
0232     <span class="comment">%case 'GMMmean'</span>
0233     <span class="comment">%    estimate(:,i) = stateMuNew*stateWNew';</span>
0234     <span class="comment">%otherwise</span>
0235     <span class="comment">%    error(' [ gmsppf ] Unknown estimate type.');</span>
0236     <span class="comment">%end</span>
0237 
0238     estimate(:,i) = XsampleBuf*sampleW';
0239 
0240     <span class="comment">%-----------------------------------------------------------------------</span>
0241     <span class="comment">% RESAMPLE</span>
0242 
0243     outIndex  = <a href="residualresample.html" class="code" title="function outIndex = residualResample(inIndex, weights);">residualresample</a>(1:numP,sampleW);
0244     XsampleBuf = XsampleBuf(:,outIndex); <span class="comment">% + eps*randn(Xdim,numP);</span>
0245     sampleW = repmat(1/numP,1,numP);
0246 
0247     <span class="comment">%-----------------------------------------------------------------------</span>
0248     <span class="comment">% Recover GMM representation of posterior distribution using EM</span>
0249 
0250     ParticleFilterDS.particles = XsampleBuf;
0251     ParticleFilterDS.weights = sampleW;
0252 
0253     stateGMM = <a href="gmmfit.html" class="code" title="function [gmmDS, leb] = gmmfit(X, M, tt, cov_type, check_cov, display, W)">gmmfit</a>(XsampleBuf, stateGMM, [0.001 10], cov_type, 1, 0);
0254 
0255     stateMu  = stateGMM.mu;
0256     stateCov = stateGMM.cov;
0257     stateW   = stateGMM.weights;
0258 
0259     <span class="keyword">if</span> pNoise.adaptMethod
0260         error(<span class="string">'  [ gmsppf ] Process noise adaptation not supported yet for GMM noise sources.'</span>);
0261     <span class="keyword">end</span>
0262 
0263     <span class="keyword">if</span> (nargout &gt; 4)
0264       extra.mu(:,:,i) = stateMu;
0265       extra.cov(:,:,:,i) = stateCov;
0266       extra.weights(:,:,i) = stateW;
0267       extra.P = zeros(Xdim,Xdim);
0268       est = estimate(:,i);
0269       <span class="keyword">for</span> kk=1:G,
0270         cS  = stateCov(:,:,kk);
0271         ttt = est-stateMu(:,kk);
0272         extra.P = extra.P + stateW(kk)*(cS*cS' + ttt*ttt');
0273       <span class="keyword">end</span>
0274     <span class="keyword">end</span>
0275 
0276 
0277 <span class="comment">%--------------------------------------------------------------------------</span>
0278 <span class="keyword">end</span>     <span class="comment">%.. loop over input vectors</span>
0279 
0280 
0281 ParticleFilterDS.stateGMM = stateGMM;
0282</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>